#include "Pano.h"

using namespace std;
using namespace cv;


void FindStereoCorners(Mat image_gray1, Mat image_gray2, vector<Point2f> &corners1, vector<Point2f> &corners2) {
	//-- Step 1: Detect the keypoints using SURF Detector
// 	SIFT sift1,sift2;
	SiftFeatureDetector sift;
	//Ptr<Feature2D> f2d = xfeatures2d::SIFT::create();
// 	Ptr<ORB> f2d = ORB::create(500,1.2f,8,31,0,2,ORB::HARRIS_SCORE,31,20);
	
	vector<KeyPoint> keypoints1, keypoints2;
	Mat descriptors1, descriptors2;
	sift.detect(image_gray1, keypoints1);
	sift.detect(image_gray2, keypoints2);
	//f2d->detect(image_gray1, keypoints1);
	//f2d->detect(image_gray2, keypoints2);
// 	sift1(image_gray1, mascara, keypoints1, descriptors1);
// 	sift2(image_gray2, mascara, keypoints2, descriptors2);
// 	cout << "keypoint num is " << keypoints1.size() << endl;

	SiftDescriptorExtractor extractor;
	extractor.compute(image_gray1, keypoints1, descriptors1);
	extractor.compute(image_gray2, keypoints2, descriptors2);
	//-- Step 2: Calculate descriptors (feature vectors)
	//f2d->compute(image_gray1, keypoints1, descriptors1);
	//f2d->compute(image_gray2, keypoints2, descriptors2);

	//-- Step 3: Matching descriptor vectors using FLANN matcher
// 	FlannBasedMatcher matcher;
	BFMatcher matcher;
// 	BruteForceMatcher<L2<float>> matcher;
	vector<DMatch> matches;
	matcher.match(descriptors1, descriptors2, matches);

	//-- Quick calculation of max and min distances between keypoints
	double max_dist = 0; double min_dist = 100;
	for (size_t i = 0; i < matches.size(); i++)
	{
		double dist = matches[i].distance;
		if (dist < min_dist) min_dist = dist;
		if (dist > max_dist) max_dist = dist;
	}
	cout << "min_dist = " << min_dist << endl;
	cout << "max_dist = " << max_dist << endl;

	//-- Use only "good" matches (i.e. whose distance is less than 3*min_dist )
	for (size_t i = 0; i < matches.size(); i++)
	{
		if (matches[i].distance < 4 * min_dist) {
// 		if (matches[i].distance < 0.9*max_dist) {
			Point2f p1 = keypoints1[matches[i].queryIdx].pt;
			Point2f p2 = keypoints2[matches[i].trainIdx].pt;
			corners1.push_back(p1);
			corners2.push_back(p2);
		}
	}
	Mat img_matches;
	drawMatches(image_gray1, keypoints1, image_gray2, keypoints2, matches, img_matches);
	cv::namedWindow("matches", cv::WINDOW_NORMAL);
	imshow("matches", img_matches);
	imwrite("matches.png", img_matches);
	cout << "corners1 size is " << corners1.size() << endl;
	cout << "corners2 size is " << corners2.size() << endl;
}


//images must be put from left to right
int Pano(Mat image1, Mat image2) {

	//find matches
	vector<Point2f> corners1, corners2;
	FindStereoCorners(image1, image2, corners1, corners2);

	cout << "conners num is " << corners1.size() << endl;
	//draw corners
	Mat image1_copy = image1.clone();
	Mat image2_copy = image2.clone();

	for (size_t i = 0; i < corners1.size(); i++)
	{
		int n1 = rand() % 255, n2 = rand() % 255, n3 = rand() % 255;
		putText(image1_copy, to_string(i), corners1[i], FONT_HERSHEY_SIMPLEX, 0.75, Scalar(n1, n2, n3), 2);
		putText(image2_copy, to_string(i), corners2[i], FONT_HERSHEY_SIMPLEX, 0.75, Scalar(n1, n2, n3), 2);
	}

	namedWindow("Corners1", CV_WINDOW_NORMAL);
	imshow("Corners1", image1_copy);
	imwrite("corners1.png", image1_copy);
	resizeWindow("Corners1", 500, 500);
	namedWindow("Corners2", CV_WINDOW_NORMAL);
	imshow("Corners2", image2_copy);
	imwrite("corners2.png", image2_copy);
	resizeWindow("Corners2", 500, 500);

	waitKey(0);

	return 0;
}